#include <ros/ros.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <nav_msgs/Odometry.h>
#include <pid_path_follow/pid_path_follow.h>

namespace pid_path_follow{
    Control::Control(){

        recv_path_flag_ = true;
        nav_flag_ = false;
        point_i_ = 0;

        ros::NodeHandle private_node("~");
        private_node.param<int>("controller_freq",controller_freq_,100);
        private_node.param<double>("linear_Kp",linear_vel_.Kp,0.0001);
        private_node.param<double>("linear_Ki",linear_vel_.Ki,0.2);
        private_node.param<double>("linear_Kd",linear_vel_.Kd,0.3);

        private_node.param<double>("Angular_Kp",angular_vel_.Kp, 0.02);
        private_node.param<double>("Angular_Ki",angular_vel_.Ki,0.2);
        private_node.param<double>("Angular_Kd",angular_vel_.Kd,0.002);

        private_node.param<double>("t",t_,5);
        private_node.param<double>("want_vel",want_vel_,0.3);
        private_node.param<bool>("loop",loop_,false);

        goal_pose_.position.x = 0.0;
        goal_pose_.position.y = 0.0;

        amcl_pose_sub_ = nh_.subscribe<geometry_msgs::PoseWithCovarianceStamped>("/amcl_pose",10,&Control::robotPoseCallBack,this);
        cmd_vel_pub_ = nh_.advertise<geometry_msgs::Twist>("/ackermann_steering_controller/cmd_vel",1);
        path_sub_ = nh_.subscribe<nav_msgs::Path>("/path_pub",1,&Control::robotPathCallBack,this);

        timer_ = nh_.createTimer(ros::Duration((1.0)/controller_freq_),&Control::controlLoopCB,this);
    }

    void Control::robotPoseCallBack(const geometry_msgs::PoseWithCovarianceStampedConstPtr& amcl_pose)
    {
        current_pose_.position.x = amcl_pose->pose.pose.position.x;
        current_pose_.position.y = amcl_pose->pose.pose.position.y;
        current_pose_.orientation = amcl_pose->pose.pose.orientation;
    }

    void Control::robotPathCallBack(const nav_msgs::PathConstPtr& path_msg)
    {
        if(recv_path_flag_)
        {
            nav_path_.poses = path_msg->poses;
            recv_path_flag_ = false;
            nav_flag_ = true;
            end_pose_.position.x = nav_path_.poses[nav_path_.poses.size()-1].pose.position.x;
            end_pose_.position.y = nav_path_.poses[nav_path_.poses.size()-1].pose.position.y;
        }
    }
    void Control::robotVelCallBack(const geometry_msgs::Twist::ConstPtr& vel_msg)
    {
        this->linear_vel_.actual_linear_vel = vel_msg->linear.x;
        this->angular_vel_.actual_angular_vel = vel_msg->angular.z;
    }
    //增量 PID
    double Control::linearPidControl(double goal_vel, LinearPID &linear)
    {
        linear.set_vel = goal_vel;
        linear.linear_err = linear.set_vel - linear.actual_linear_vel;
        double increment = linear.Kp* (linear.linear_err) + linear.Ki * linear.linear_err 
                            + linear.Kd * (linear.linear_err - 2 * linear.linear_err_last + linear.linear_err_l_last);

        linear.actual_linear_vel += increment ;
        linear.linear_err_l_last = linear.linear_err_last;
        linear.linear_err_last = linear.linear_err;
        return linear.actual_linear_vel;
    }
    //位置 PID
    double Control::angularPidControl(double angular_vel, AngularPID &angular)
    {
        angular.set_angualr_vel = angular_vel;
        angular.angular_err = angular.set_angualr_vel - angular.actual_angular_vel;
        angular.integral+= angular.angular_err;
        double voltage = angular.Kp*angular.angular_err + angular.Ki* angular.integral + angular.Kd*(angular.angular_err - angular.angualr_err_last);
        angular.angualr_err_last = angular.angular_err;
        angular.actual_angular_vel = voltage * 1.0;
        return angular.actual_angular_vel;
    }
    //
    double Control::calculate_angular()
    {
        //目标点 在机器人坐标系下的向量
        double corrd_x1 = goal_pose_.position.x - current_pose_.position.x;
        double corrd_y1 = goal_pose_.position.y - current_pose_.position.y;

        tf::Quaternion quat;
        tf::quaternionMsgToTF(current_pose_.orientation,quat);
        double roll, pitch, yaw;
        tf::Matrix3x3(quat).getRPY(roll,pitch,yaw);
        //机器人朝向
        double corrd_x2 = std::cos(yaw);
        double corrd_y2 = std::sin(yaw);
        //机器人的转向角
        double Dot_Product = corrd_x1 * corrd_x2 + corrd_y1 * corrd_y2; //点乘
        double temp = Dot_Product / std::sqrt(pow(corrd_x1,2)+pow(corrd_y1,2)); //夹角余铉
        double alpha = std::acos(fminl(fmaxl(temp,-1.0),1.0));  //计算夹角
        //叉乘判断方向，正右转 ，负左转
        double Product = corrd_x1 * corrd_y2 - corrd_x2 * corrd_y1; //叉乘正负决定转向
        if(Product > 0) alpha = -alpha;
        return alpha;
    }
    void Control::calculate_vel()
    {
        update_point();
        cmd_vel_.linear.x = linearPidControl(want_vel_,linear_vel_);
        double w = calculate_angular() * t_;
        cmd_vel_.angular.z = angularPidControl(w, angular_vel_);
        double dis = distance(current_pose_.position.x, current_pose_.position.y,end_pose_.position.x,end_pose_.position.y);
	if(loop_ == false)    
	{    
		if(dis < 0.1 && point_i_ > 10)
		{
		    cmd_vel_.linear.x = 0;
		    cmd_vel_.angular.z = 0;
		}
	}

    }
    void Control::update_point()
    {
        if(nav_flag_)
        {
            nav_flag_ = true;
            goal_pose_.position.x = nav_path_.poses[point_i_].pose.position.x;
            goal_pose_.position.y = nav_path_.poses[point_i_].pose.position.y;
            double dis = distance(current_pose_.position.x, current_pose_.position.y, goal_pose_.position.x, goal_pose_.position.y);
            if(dis < 0.2)
                point_i_ ++;
            if(loop_ )
            {
                double dis_end = distance(current_pose_.position.x, current_pose_.position.y,end_pose_.position.x,end_pose_.position.y);
                if(dis_end < 0.1)
                    point_i_ = 0;
            }
        }
    }
    void Control::controlLoopCB(const ros::TimerEvent&)
    {
        calculate_vel();
        cmd_vel_pub_.publish(cmd_vel_);
    }


}
int main(int argc,char** argv)
{
    ros::init(argc,argv,"pid_path_follow");
    pid_path_follow::Control control;
    ros::AsyncSpinner spinner(2);
    spinner.start();
    ros::waitForShutdown();
    return 0;
}

